#include "dji_linux_helpers.hpp"
#include "dji_linker.hpp"
#include "GSLAM/core/GSLAM.h"
#include "flight_sample.hpp"
#include "flight_control_sample.hpp"
#include <dji_vehicle.hpp>

using namespace DJI::OSDK;
using namespace DJI::OSDK::Telemetry;


using namespace GSLAM;
using namespace sv;


void ObtainJoystickCtrlAuthorityCB(ErrorCode::ErrorCodeType errorCode, UserData userData)
{
  if (errorCode == ErrorCode::FlightControllerErr::SetControlParam::ObtainJoystickCtrlAuthoritySuccess)
  {
    std::cout << "ObtainJoystickCtrlAuthoritySuccess" << std::endl;
  }
}

void ReleaseJoystickCtrlAuthorityCB(ErrorCode::ErrorCodeType errorCode, UserData userData)
{
  if (errorCode == ErrorCode::FlightControllerErr::SetControlParam::ReleaseJoystickCtrlAuthoritySuccess)
  {
    std::cout << "ReleaseJoystickCtrlAuthoritySuccess" << std::endl;
  }
}

class FlightControl{
public:
    FlightControl(Vehicle* vehicle)
        : vehicle(vehicle){
        initialize();
    }
    ~FlightControl(){}

    void initialize(){
        int functionTimeout = 1;
        vehicle->flightController->obtainJoystickCtrlAuthorityAsync(ObtainJoystickCtrlAuthorityCB, nullptr ,functionTimeout, 2);

    }

    void set_flight_position(float xOffsetDesired=0.0, float yOffsetDesired=0.0, float zOffsetDesired=0.0,
                             float yawDesired=0.0, float posThresholdInM=0.0, float yawThresholdInDeg=0.0){
        moveByPositionOffset(vehicle,
                             xOffsetDesired, yOffsetDesired, zOffsetDesired,
                             yawDesired, posThresholdInM, yawThresholdInDeg);
    }

    void set_flight_speed(float offsetDesired_x, float offsetDesired_y, float offsetDesired_z, float yawRate,int timeMs){
        uint32_t originTime  = 0;
        uint32_t currentTime = 0;
        uint32_t elapsedTimeInMs = 0;
        OsdkOsal_GetTimeMs(&originTime);
        OsdkOsal_GetTimeMs(&currentTime);
        elapsedTimeInMs = currentTime - originTime;

        FlightController::JoystickMode joystickMode = {
          FlightController::HorizontalLogic::HORIZONTAL_VELOCITY,
          FlightController::VerticalLogic::VERTICAL_VELOCITY,
          FlightController::YawLogic::YAW_RATE,
          FlightController::HorizontalCoordinate::HORIZONTAL_GROUND,
          FlightController::StableMode::STABLE_ENABLE,
        };

        vehicle->flightController->setJoystickMode(joystickMode);
        FlightController::JoystickCommand joystickCommand = {offsetDesired_x, offsetDesired_y, offsetDesired_z,yawRate};
        vehicle->flightController->setJoystickCommand(joystickCommand);

        while(elapsedTimeInMs <= timeMs)
        {
          vehicle->flightController->joystickAction();
          usleep(20000);
          OsdkOsal_GetTimeMs(&currentTime);
          elapsedTimeInMs = currentTime - originTime;
        }
    }


public:
    Vehicle* vehicle;


};


REGISTER_SVAR_MODULE(FlightControl){
    Class<FlightControl>("FlightControl")
            .unique_construct<Vehicle*>()
            .def("set_flight_position",&FlightControl::set_flight_position)
            .def("set_flight_speed",&FlightControl::set_flight_speed);

    Class<float>("float")
            .construct<double>();

    Class<double>("double")
            .def("__float__",[](double self){return float(self);});

}
